//dll ws2_32.dll
#include <winsock2.h>
#include <process.h>
#include <stdio.h>
#include <stdlib.h>

#include "commands.h"
#include "STLinkUSBDriver.h"

#define DLLNAME "stlink.dll"
#define UDPTimeout 3000000

void *handle = 0;

typedef uint32_t(STD_CALL *pSTLink_Reenumerate)(TEnumStlinkInterface IfId, uint8_t bClearList) __attribute__((stdcall));

typedef uint32_t(STD_CALL *pSTLink_GetNbDevices)(TEnumStlinkInterface IfId) __attribute__((stdcall));

typedef uint32_t(STD_CALL *pSTLink_GetDeviceInfo)(TEnumStlinkInterface IfId,
                                                  uint8_t DevIdxInList, TDeviceInfo *pInfo, uint32_t InfoSize) __attribute__((stdcall));

typedef uint32_t(STD_CALL *pSTLink_GetDeviceInfo2)(TEnumStlinkInterface IfId,
                                                   uint8_t DevIdxInList, TDeviceInfo2 *pInfo, uint32_t InfoSize) __attribute__((stdcall));

typedef uint32_t(STD_CALL *pSTLink_OpenDevice)(TEnumStlinkInterface IfId,
                                               uint8_t DevIdxInList, uint8_t bExclusiveAccess, void **pHandle) __attribute__((stdcall));

typedef uint32_t(STD_CALL *pSTLink_CloseDevice)(void *pHandle) __attribute__((stdcall));

typedef uint32_t(STD_CALL *pSTLink_SendCommand)(void *pHandle,
                                                PDeviceRequest pRequest, uint32_t DwTimeOut) __attribute__((stdcall));

typedef uint32_t(STD_CALL *pSTLink_ReenumerateTcp)(TEnumStlinkInterface IfId, uint8_t bClearList,
                                                   char *pConnectParams, char *pServerCmdLineParams) __attribute__((stdcall));

typedef uint32_t(STD_CALL *pSTLink_OpenDeviceTcp)(TEnumStlinkInterface IfId,
                                                  uint32_t DevInfoUsbId, uint8_t bExclusiveAccess) __attribute__((stdcall));

typedef uint32_t(STD_CALL *pSTLink_CloseDeviceTcp)(uint32_t StLinkUsbId, uint8_t closeTcp) __attribute__((stdcall));

typedef uint32_t(STD_CALL *pSTLink_SendCommandTcp)(uint32_t StLinkUsbId,
                                                   PDeviceRequest pRequest, uint32_t DwTimeOut) __attribute__((stdcall));

typedef uint32_t(STD_CALL *pSTLink_GetNumOfDeviceClientsTcp)(uint32_t StLinkUsbId) __attribute__((stdcall));

//-----------------------stmass

typedef uint32_t(STD_CALL *pSTMass_Enum_GetNbDevices)() __attribute__((stdcall));

typedef uint32_t(STD_CALL *pSTMass_Enum_GetDevice)(uint32_t id, void *p) __attribute__((stdcall));

typedef uint32_t(STD_CALL *pSTMass_GetDeviceInfo)(int a1, int a2, char *Destination, int SizeInBytes) __attribute__((stdcall));

typedef uint32_t(STD_CALL *pSTMass_OpenDevice)(int *InterfaceHandle, int *a2) __attribute__((stdcall));

typedef uint32_t(STD_CALL *pSTMass_OpenDeviceExclusive)(int *InterfaceHandle, int *a2) __attribute__((stdcall));

typedef uint32_t(STD_CALL *pSTMass_CloseDevice)(int a1, int *hObject) __attribute__((stdcall));

typedef uint32_t(STD_CALL *pSTMass_SendCommand)(int id, void *pHandle, PDeviceRequest pRequest, uint32_t DwTimeOut) __attribute__((stdcall));

typedef uint32_t(STD_CALL *pSTMass_Enum_Reenumerate)() __attribute__((stdcall));

const char *STLINK_PROXYNAME[] = {
    "STLink_Reenumerate",
    "STLink_GetNbDevices",
    "STLink_GetDeviceInfo",
    "STLink_GetDeviceInfo2",
    "STLink_OpenDevice",
    "STLink_CloseDevice",
    "STLink_SendCommand",

    "STLink_ReenumerateTcp",
    "STLink_OpenDeviceTcp",
    "STLink_CloseDeviceTcp",
    "STLink_SendCommandTcp",
    "STLink_GetNumOfDeviceClientsTcp",

    "STMass_Enum_GetNbDevices",
    "STMass_Enum_GetDevice",
    "STMass_GetDeviceInfo",
    "STMass_OpenDevice",
    "STMass_OpenDeviceExclusive",
    "STMass_CloseDevice",
    "STMass_SendCommand",
    "STMass_Enum_Reenumerate",
};

typedef struct
{
    pSTLink_Reenumerate STLink_Reenumerate;
    pSTLink_GetNbDevices STLink_GetNbDevices;
    pSTLink_GetDeviceInfo STLink_GetDeviceInfo;
    pSTLink_GetDeviceInfo2 STLink_GetDeviceInfo2;
    pSTLink_OpenDevice STLink_OpenDevice;
    pSTLink_CloseDevice STLink_CloseDevice;
    pSTLink_SendCommand STLink_SendCommand;

    pSTLink_ReenumerateTcp STLink_ReenumerateTcp;
    pSTLink_OpenDeviceTcp STLink_OpenDeviceTcp;
    pSTLink_CloseDeviceTcp STLink_CloseDeviceTcp;
    pSTLink_SendCommandTcp STLink_SendCommandTcp;
    pSTLink_GetNumOfDeviceClientsTcp STLink_GetNumOfDeviceClientsTcp;

    pSTMass_Enum_GetNbDevices STMass_Enum_GetNbDevices;
    pSTMass_Enum_GetDevice STMass_Enum_GetDevice;
    pSTMass_GetDeviceInfo STMass_GetDeviceInfo;
    pSTMass_OpenDevice STMass_OpenDevice;
    pSTMass_OpenDeviceExclusive STMass_OpenDeviceExclusive;
    pSTMass_CloseDevice STMass_CloseDevice;
    pSTMass_SendCommand STMass_SendCommand;
    pSTMass_Enum_Reenumerate STMass_Enum_Reenumerate;

} STLink;

const char Devinfo[]={0x00,0x00,0x00,0x00,0x33,0x39,0x46,0x46,0x36,0x38,0x30,0x36,0x34,0x31,0x34,0x33,0x33,0x35,0x33,0x31,0x35,0x33,0x34,0x35,0x30,0x39,0x35,0x37,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x83,0x04,0x48,0x37,0x00};
STLink STLinkHandle;
FILE *fp;
void addlog(int i)
{
    if (!fp)
        return;
    fputs(STLINK_PROXYNAME[i], fp);
    fputs("\n", fp);
    fflush(fp);
}
void addstr(char *str)
{
    if (!fp)
        return;
    fputs(str, fp);
    fflush(fp);
}

const char *defstr[256];
char defuse[256];
int starttime = 0;
void initdefstr()
{
    int i;
    for (i = 0; i < 256; i++)
    {
        defstr[i] = "undef";
        defuse[i] = 0;
    }
    starttime = time(0);
    defstr[0x01] = "GETSTATUS";
    defstr[0x02] = "FORCEDEBUG";
    defstr[0x03] = "APIV1_RESETSYS";
    defstr[0x04] = "APIV1_READALLREGS";
    defstr[0x05] = "APIV1_READREG";
    defstr[0x06] = "APIV1_WRITEREG";
    defstr[0x07] = "READMEM_32BIT";
    defuse[0x7] = 1;
    defstr[0x08] = "WRITEMEM_32BIT";
    defuse[0x8] = 1;
    defstr[0x09] = "RUNCORE";
    defstr[0x0a] = "STEPCORE";
    defstr[0x0b] = "APIV1_SETFP";
    defstr[0x0c] = "READMEM_8BIT";
    defstr[0x0d] = "WRITEMEM_8BIT";
    defstr[0x0e] = "APIV1_CLEARFP";
    defstr[0x0f] = "APIV1_WRITEDEBUGREG";
    defstr[0x10] = "APIV1_SETWATCHPOINT";
    defstr[0x00] = "ENTER_JTAG_RESET";
    defstr[0xa3] = "ENTER_SWD_NO_RESET";
    defstr[0xa4] = "ENTER_JTAG_NO_RESET";
    defstr[0x20] = "APIV1_ENTER";
    defstr[0x21] = "EXIT";
    defstr[0x22] = "READCOREID";
    defstr[0x30] = "APIV2_ENTER";
    defstr[0x31] = "APIV2_READ_IDCODES";
    defstr[0x32] = "APIV2_RESETSYS";
    defstr[0x33] = "APIV2_READREG";
    defuse[0x33] = 1;
    defstr[0x34] = "APIV2_WRITEREG";
    defuse[0x34] = 1;
    defstr[0x35] = "APIV2_WRITEDEBUGREG";
    defuse[0x35] = 1;
    defstr[0x36] = "APIV2_READDEBUGREG";
    defuse[0x36] = 1;
    defstr[0x3A] = "APIV2_READALLREGS";
    defuse[0x3a] = 1;
    defstr[0x3B] = "APIV2_GETLASTRWSTATUS";
    defstr[0x3C] = "APIV2_DRIVE_NRST";
    defstr[0x3E] = "APIV2_GETLASTRWSTATUS2";
    defuse[0x3e] = 1;
    defstr[0x40] = "APIV2_START_TRACE_RX";
    defstr[0x41] = "APIV2_STOP_TRACE_RX";
    defstr[0x42] = "APIV2_GET_TRACE_NB";
    defstr[0x43] = "APIV2_SWD_SET_FREQ";
    defstr[0x44] = "APIV2_JTAG_SET_FREQ";
    defstr[0x45] = "APIV2_READ_DAP_REG";
    defstr[0x46] = "APIV2_WRITE_DAP_REG";
    defstr[0x47] = "APIV2_READMEM_16BIT";
    defstr[0x48] = "APIV2_WRITEMEM_16BIT";
    defstr[0x4B] = "APIV2_INIT_AP";
    defstr[0x4C] = "APIV2_CLOSE_AP_DBG";
    defstr[0x50] = "WRITEMEM_32BIT_NO_ADDR_IN";
    defstr[0x51] = "APIV2_RW_MISC_OUT";
    defstr[0x52] = "APIV2_RW_MISC_IN";
    defstr[0x54] = "READMEM_32BIT_NO_ADDR_INC";
    defstr[0x61] = "SET_COM_FREQ";
    defstr[0x62] = "GET_COM_FREQ";
    defstr[0xF1] = "GET_VERSION";
    defstr[0xF2] = "DEBUG_COMMAND";
    defstr[0xF3] = "DFU_COMMAND";
    defstr[0xF5] = "GET_CURRENT_MODE";
    defstr[0xF7] = "GET_TARGET_VOLTAGE";
    defstr[0xFB] = "GET_VERSION_APIV3";
}

void adderror()
{
    if (!fp)
        return;
    char buff[64];
    sprintf(buff, "error:%d\r\n", GetLastError());
    fputs(buff, fp);
    fflush(fp);
}

SOCKET sock = 0;
HANDLE h1;
struct sockaddr* lastTarget;
struct sockaddr_in Target;
int Targetlen=sizeof(struct sockaddr);
int iplist[16];
int ipcount = 16;
char remote=0;

int getIPs(int *ips, int *count)
{
    char buff[64];
    int i;
    struct sockaddr_in sa;
    int c = 0;
    gethostname(&buff, sizeof(buff));
    struct hostent *phost = gethostbyname(&buff);
    for (i = 0; phost->h_addr_list[i]; i++)
    {
        *ips++ = *((int *)phost->h_addr_list[i]);
        c++;
    }
    *count = c;
}
char *recvdata;
int recvlen;
void test(void *v)
{
    char buff[10240];
    char sbuff[256];
    int len;
    while (sock)
    {
        len = recvfrom(sock, buff, sizeof(buff), 0,(struct sockaddr *)&Target,&Targetlen);
        lastTarget=&Target;
        recvdata=buff;
        recvlen=len;
        // sprintf(sbuff, "r:%d\n", len);
        // fputs(sbuff, fp);
        // fflush(fp);
        if (0xFFFFFFFF == (*(int *)buff))
            remote=1;
        Sleep(0);
        //fputs("recvnext\n",fp);
    }
    if (sock)
    {
        closesocket(sock);
        sock=0;
    }
    // printf("thread end\r\n");
}

void udpscan(){
    struct sockaddr_in st;
    st.sin_family = AF_INET;
    st.sin_port = htons(51);
    for (int i = 0; i < ipcount; i++)
    {
        st.sin_addr.S_un.S_addr = iplist[i] | 0xff000000;
        sendto(sock, "te", 2, 0, (const struct sockaddr *)&st, sizeof(st));
    }
}

void initudp()
{
    WSADATA data;
    WSAStartup(MAKEWORD(2, 2), &data);
    getIPs(&iplist, &ipcount);
    struct sockaddr_in si;
    
    si.sin_family = AF_INET;
    si.sin_port = htons(52);
    si.sin_addr.S_un.S_addr = ADDR_ANY;

    sock = WSASocket(AF_INET, SOCK_DGRAM, IPPROTO_UDP, NULL, 0, WSA_FLAG_OVERLAPPED);
    bind(sock, (const struct sockaddr *)&si, sizeof(si));
    h1 = _beginthread(test, 0, NULL);
    udpscan();
}

typedef struct packetout
{
    int len;
    uint8_t CDBByte[16]; // Command header (CDB on mass storage)
};

char writebuff[10240];
int wpos=0;
void w8(char b){
    writebuff[wpos++]=b;
}
void w16(int b){
    writebuff[wpos++]=b&0xff;
    writebuff[wpos++]=(b>>8)&0xff;
}
void w32(int b){
    writebuff[wpos++]=b&0xff;
    writebuff[wpos++]=(b>>8)&0xff;
    writebuff[wpos++]=(b>>16)&0xff;
    writebuff[wpos++]=(b>>24)&0xff;
}

int buildpacket(PDeviceRequest p){
    int i;
    int len=p->CDBLength?p->CDBLength:12;
    int rlen=p->BufferLength;
    char *pb=(char*)p->Buffer;
    // char buff[64];
    // sprintf(buff, "%d->[%x-%x]\n", time(0) - starttime, p->CDBByte[0],p->CDBByte[1]);
    // fputs(buff, fp);
    // fflush(fp);
    wpos=0;
    w8(len);
    w8(p->InputRequest);
    w16(rlen);
    for(i=0;i<len;i++)
        w8(p->CDBByte[i]);
    if(!p->InputRequest)
        for(i=0;i<p->BufferLength;i++)
            w8(pb[i]);
    len=p->InputRequest?4+len:4+len+rlen;
    recvlen=0;
    sendto(sock,writebuff,len,0,lastTarget,&Targetlen);
    if(!p->InputRequest)return;
    if(!remote)return 1;
    int count=0;
    int resend=0;
    while(1){
        if(++count>UDPTimeout)break;
        // if(++resend>10000){
        //     resend=0;
        //     sendto(sock,writebuff,len,0,lastTarget,&Targetlen);
        // }
        if (recvlen != 0)
        {
            for (i = 0; i < p->BufferLength; i++)
                pb[i] = recvdata[i];
            return 1;
        }
        Sleep(0);
    }
    fputs("recvtimeout\n",fp);
    return 0x1001;
}

void addtdp(PDeviceRequest p)
{
    if (!fp)
        return;
    char buff[128];
    char *x = &p->CDBByte;
    x += 2;
    unsigned int *px = (int *)x;
    sprintf(buff, "%d->[%x-%x]\n", time(0) - starttime, p->CDBByte[0],p->CDBByte[1]);
    fputs(buff, fp);
    fflush(fp);
    if(lastTarget)
        buildpacket(p); 
    if (p->CDBByte[0] == 0xf2 && defuse[p->CDBByte[1]])
    {
        sprintf(buff, "%d->[%x]\n", time(0) - starttime, p->CDBByte[1]);
        fputs(buff, fp);
        fflush(fp);
        return;
    }
    sprintf(buff, "%d->Write:%d RW:%d EP:%d 0x%08x 0x%08x\n", time(0) - starttime, p->CDBLength, p->BufferLength, p->InputRequest, px[0], px[1]);
    fputs(buff, fp);
    sprintf(buff, "->%s[%x]\n", defstr[p->CDBByte[0]], p->CDBByte[0]);
    fputs(buff, fp);
    sprintf(buff, "->%s[%x]\n", defstr[p->CDBByte[1]], p->CDBByte[1]);
    fputs(buff, fp);
    sprintf(buff, "->%s[%x]\n", defstr[p->CDBByte[2]], p->CDBByte[2]);
    fputs(buff, fp);
    if (p->BufferLength)
    {
        sprintf(buff, "->Ret:0x%08x 0x%08x 0x%08x\n", ((int *)p->Buffer)[0], p->BufferLength > 4 ? ((int *)p->Buffer)[1] : 0, p->BufferLength > 8 ? ((int *)p->Buffer)[2] : 0);
        fputs(buff, fp);
    }
    fflush(fp);
}

char *libpath = 0;
int Init()
{
    int i = 0;
    if (STLinkHandle.STLink_Reenumerate)
        return 1;
    if (!fp)
        fp = fopen("z:\\log.txt", "a");
    initdefstr();
    
    int *ptr = &STLinkHandle;
    // int* hd=GetModuleHandle("STLinkUSBDriver.dll");
    addstr("loader start\n");
    int *hd = LoadLibraryEx(libpath ? libpath : DLLNAME, NULL, LOAD_WITH_ALTERED_SEARCH_PATH);
    if (!hd)
    {
        adderror();
        return 0;
    }
    addstr("loader ok\n");
    for (i = 0; i < sizeof(STLINK_PROXYNAME) / 4; i++)
        ptr[i] = GetProcAddress(hd, STLINK_PROXYNAME[i]);
    
    initudp();
    return 1;
}

__declspec(dllexport) uint32_t __stdcall STLink_Reenumerate(TEnumStlinkInterface IfId, uint8_t bClearList)
{
    Init();
    if(remote)return 1;
    addlog(0);
    int r = STLinkHandle.STLink_Reenumerate(IfId, bClearList);
    
    
    if(!lastTarget)
        udpscan();
    return r;
}

__declspec(dllexport) uint32_t __stdcall STLink_GetNbDevices(TEnumStlinkInterface IfId)
{
    Init();
    if(remote)return 1;
    addlog(1);
    int r = STLinkHandle.STLink_GetNbDevices(IfId);
    
    return r;
}

__declspec(dllexport) uint32_t __stdcall STLink_GetDeviceInfo(TEnumStlinkInterface IfId,
                                                              uint8_t DevIdxInList, TDeviceInfo *pInfo, uint32_t InfoSize)
{
    Init();
    int r = STLinkHandle.STLink_GetDeviceInfo(IfId, DevIdxInList, pInfo, InfoSize);
    addlog(2);
    return r;
}

__declspec(dllexport) uint32_t __stdcall STLink_GetDeviceInfo2(TEnumStlinkInterface IfId,
                                                               uint8_t DevIdxInList, TDeviceInfo2 *pInfo, uint32_t InfoSize)
{
    char buff[64];
    Init();
    if(remote){
        char* p=(char*)pInfo;
        int i;
        for(i=0;i<41;i++)
            p[i]=Devinfo[i];
        return 1;
    }
    addlog(3);
    int r = STLinkHandle.STLink_GetDeviceInfo2(IfId, DevIdxInList, pInfo, InfoSize);
    
    sprintf(buff,"id %d dev %d sn %s\r\n",IfId,DevIdxInList,pInfo->EnumUniqueId);
    fputs(buff,fp);
    fflush(fp);
    
    return r;
}

__declspec(dllexport) uint32_t __stdcall STLink_OpenDevice(TEnumStlinkInterface IfId,
                                                           uint8_t DevIdxInList, uint8_t bExclusiveAccess, void **pHandle)
{
    Init();
    if(remote){
        *pHandle=&STLinkHandle;
        return 1;
    }
    addlog(4);
    int r = STLinkHandle.STLink_OpenDevice(IfId, DevIdxInList, bExclusiveAccess, pHandle);
    
    return r;
}

__declspec(dllexport) uint32_t __stdcall STLink_CloseDevice(void *pHandle)
{
    Init();
    if(remote){
        return 1;
    }
    int r = STLinkHandle.STLink_CloseDevice(pHandle);
    addlog(5);
    
    return r;
}

__declspec(dllexport) uint32_t __stdcall STLink_SendCommand(void *pHandle,
                                                            PDeviceRequest pRequest, uint32_t DwTimeOut)
{
    char buff[64];
    Init();
    if(remote){
        //addtdp(pRequest);
        // fputs("recvok\n",fp);
        // fflush(fp);
        buildpacket(pRequest); 
        return 1;
    }
    int r = STLinkHandle.STLink_SendCommand(pHandle, pRequest, DwTimeOut);
    addlog(6);
    addtdp(pRequest);
    return r;
}
////-----------------------------------------

__declspec(dllexport) uint32_t __stdcall STLink_ReenumerateTcp(TEnumStlinkInterface IfId, uint8_t bClearList,
                                                               char *pConnectParams, char *pServerCmdLineParams)
{
    addlog(7);
    return STLinkHandle.STLink_ReenumerateTcp(IfId, bClearList, pConnectParams, pServerCmdLineParams);
}

__declspec(dllexport) uint32_t __stdcall STLink_OpenDeviceTcp(TEnumStlinkInterface IfId,
                                                              uint32_t DevInfoUsbId, uint8_t bExclusiveAccess)
{
    addlog(8);
    return STLinkHandle.STLink_OpenDeviceTcp(IfId, DevInfoUsbId, bExclusiveAccess);
}

__declspec(dllexport) uint32_t __stdcall STLink_CloseDeviceTcp(uint32_t StLinkUsbId, uint8_t closeTcp)
{
    addlog(9);
    return STLinkHandle.STLink_CloseDeviceTcp(StLinkUsbId, closeTcp);
}

__declspec(dllexport) uint32_t __stdcall STLink_SendCommandTcp(uint32_t StLinkUsbId,
                                                               PDeviceRequest pRequest, uint32_t DwTimeOut)
{
    addlog(10);
    return STLinkHandle.STLink_SendCommandTcp(StLinkUsbId, pRequest, DwTimeOut);
}

__declspec(dllexport) uint32_t __stdcall STLink_GetNumOfDeviceClientsTcp(uint32_t StLinkUsbId)
{
    addlog(11);
    return STLinkHandle.STLink_GetNumOfDeviceClientsTcp(StLinkUsbId);
}

///------------------------------------------
__declspec(dllexport) uint32_t __stdcall STMass_Enum_GetNbDevices()
{
    addlog(12);
    return STLink_GetNbDevices(STLINK_DBG_INTERFACE);
}

__declspec(dllexport) uint32_t __stdcall STMass_Enum_GetDevice(uint32_t id, void *p)
{
    addlog(13);
    return STLinkHandle.STMass_Enum_GetDevice(id, p);
}

__declspec(dllexport) uint32_t __stdcall STMass_GetDeviceInfo(int a1, int a2, char *Destination, int SizeInBytes)
{
    addlog(14);
    return STLinkHandle.STMass_GetDeviceInfo(a1, a2, Destination, SizeInBytes);
}

__declspec(dllexport) uint32_t __stdcall STMass_OpenDevice(int *InterfaceHandle, int *a2)
{
    addlog(15);
    return STLinkHandle.STMass_OpenDevice(InterfaceHandle, a2);
}

__declspec(dllexport) uint32_t __stdcall STMass_OpenDeviceExclusive(int *InterfaceHandle, int *a2)
{
    addlog(16);
    return STLinkHandle.STMass_OpenDeviceExclusive(InterfaceHandle, a2);
}

__declspec(dllexport) uint32_t __stdcall STMass_CloseDevice(int a1, int *hObject)
{
    addlog(17);
    return STLinkHandle.STMass_CloseDevice(a1, hObject);
}

__declspec(dllexport) uint32_t __stdcall STMass_SendCommand(int id, void *pHandle, PDeviceRequest pRequest, uint32_t DwTimeOut)
{
    addlog(18);
    return STLinkHandle.STMass_SendCommand(id, pHandle, pRequest, DwTimeOut);
}

__declspec(dllexport) uint32_t __stdcall STMass_Enum_Reenumerate()
{
    addlog(19);
    return STLinkHandle.STLink_Reenumerate(STLINK_DBG_INTERFACE, 0);
}

const char *str[] = {
    "DLL_PROCESS_DETACH",
    "DLL_PROCESS_ATTACH",
    "DLL_THREAD_ATTACH",
    "DLL_THREAD_DETACH,",
    "DLL_PROCESS_VERIFIER"};

void toTarget(char *p, char *t)
{
    int i;
    char *s = p;
    int px = 0;
    while (*s)
        px = (*s++ == '\\') ? (s - p) : px;
    s = p + px;
    while (*t)
        *s++ = *t++;
    *s = 0;
}



int WINAPI DllMain(
    HINSTANCE hInstance,
    DWORD fdwReason,
    LPVOID lpvReserved)
{
    if (fdwReason == 0)
    {
    }
    if (fdwReason == 1)
    {
        char buff[260];
        GetModuleFileName(hInstance, &buff, sizeof(buff));
        libpath = &buff;
        toTarget(libpath, DLLNAME);
        Init();
        libpath = 0;
    }
    return Init();
}
main()
{
    int i;
    TDeviceInfo2 *info;
    char data[43] = {0};
    PDeviceRequest pdata;
    long long *pc;
    uint8_t recv[16] = {0};
    char rd[41] = {0};
    // uint8_t data[256];
    printf("%d", time(0));
    if (!Init())
        return;
    printf("dll STLink_Reenumerate:0x%x\r\n", STLink_Reenumerate(STLINK_DBG_INTERFACE, 0));
    printf("STLink_Reenumerate:%d\r\n", STLinkHandle.STLink_Reenumerate);
    pdata = &data;
    pc = &pdata->CDBByte;

    printf("void* %d\r\n", sizeof(void *));
    printf("TDeviceRequest %d\r\n", sizeof(TDeviceRequest));
    printf("TDeviceInfo2 %d\r\n", sizeof(TDeviceInfo2));
    printf("%x %x %x %x %x\r\n", &pdata->CDBLength, &pdata->CDBByte[0], &pdata->InputRequest, &pdata->Buffer, &pdata->BufferLength);
    printf("STLink_Reenumerate:0x%x\r\n", STLinkHandle.STLink_Reenumerate(STLINK_DBG_INTERFACE, 0));
    printf("STLink_GetNbDevices:%d\r\n", STLinkHandle.STLink_GetNbDevices(STLINK_DBG_INTERFACE));
    printf("STLink_GetDeviceInfo:%d\r\n", STLinkHandle.STLink_GetDeviceInfo2(STLINK_DBG_INTERFACE, 0, (TDeviceInfo2 *)rd, 41));
    for(i=0;i<41;i++){
        printf("0x%02x,",(unsigned char)rd[i]);
    }
    printf("\r\n");
    info = (TDeviceInfo2 *)rd;
    printf("Bridge %d PID: 0X%04hx SN:%s Use:%s\n", (int)0, (unsigned short)info->ProductId, info->EnumUniqueId, info->DeviceUsed ? "true" : "false");
    printf("STLink_OpenDevice:0x%x handle:0x%x\r\n", STLinkHandle.STLink_OpenDevice(STLINK_DBG_INTERFACE, 0, 0, &handle), handle);

    pdata->CDBLength = STLINK_CMD_SIZE_16;
    pdata->CDBByte[0] = STLINK_GET_VERSION;
    pdata->CDBByte[1] = 0x80;
    pdata->InputRequest = REQUEST_READ_1ST_EPIN;
    pdata->Buffer = recv;
    pdata->BufferLength = 16;
    pdata->SenseLength = DEFAULT_SENSE_LEN;

    printf("STLink_SendCommand:0x%x\r\n", STLinkHandle.STLink_SendCommand(handle, pdata, 500));
    printf("stlink %d,jtag %d,swim %d\r\n", ((recv[0] & 0xf0) >> 4), ((recv[0] & 0x0f) << 2) | ((recv[1] & 0xc0) >> 6), recv[1] & 0x3f);

    pdata->CDBByte[0] = STLINK_DFU_COMMAND;
    pdata->CDBByte[1] = STLINK_DFU_EXIT;
    pdata->BufferLength = 0;
    printf("STLink_SendCommand:0x%x\r\n", STLinkHandle.STLink_SendCommand(handle, pdata, 500));

    pdata->CDBByte[0] = STLINK_GET_CURRENT_MODE;
    pdata->BufferLength = 2;
    printf("STLink_SendCommand:0x%x\r\n", STLinkHandle.STLink_SendCommand(handle, pdata, 500));
    printf("Mode:0x%x\r\n", recv[0]);

    pdata->CDBByte[0] = STLINK_DEBUG_COMMAND;
    pdata->CDBByte[1] = STLINK_DEBUG_APIV2_ENTER;
    pdata->CDBByte[2] = STLINK_DEBUG_ENTER_SWD_NO_RESET;
    pdata->BufferLength = 2;
    printf("STLink_SendCommand:0x%x\r\n", STLinkHandle.STLink_SendCommand(handle, pdata, 500));

    pdata->CDBByte[0] = STLINK_GET_CURRENT_MODE;
    pdata->BufferLength = 2;
    printf("STLink_SendCommand:0x%x\r\n", STLinkHandle.STLink_SendCommand(handle, pdata, 500));
    printf("Mode:0x%x\r\n", recv[0]);

    pdata->CDBByte[0] = STLINK_DEBUG_COMMAND;
    pdata->CDBByte[1] = STLINK_DEBUG_READCOREID;
    pdata->BufferLength = 4;
    printf("STLink_SendCommand:0x%x\r\n", STLinkHandle.STLink_SendCommand(handle, pdata, 500));
    printf("CoreID:0x%x\r\n", *(uint32_t *)recv);

    if (handle != 0)
        printf("STLink_CloseDevice:0x%x\r\n", STLinkHandle.STLink_CloseDevice(handle));
    // printf("name:%s\r\n",info.DevPath);
}